function P=KF_step(P)

% KF_step: Kalman filter step
%
% SYNTAX:
%
%   P = KF_step(P)
%   simulates a Kalman filter step for MADS.Player object P and stores results in P 
%
% System model: discrete-time linear time-invariant or time-varying system
%       x[t] = A[t] x[t-1] + B[t] u[t] + w[t]
%       y[t] = C[t] x[t] + D[t] u[t] + v[t]
%   where Gaussian noise w[t] and v[t] obey N(0,Q[t]) and N(0,R[t]), respectively
%   
% Kalman Filter Algorithm:
%
%   time update:        
%       x[t|t-1] = A[t] x[t-1|t-1] + B[t] u[t]
%       P[t|t-1] =  A[t] P[t-1|t-1] A[t]' + B[t]*Q[t]*B[t]'
%   observation update: 
%       L[t] = P[t|t-1] C[t]' inv(C[t] P[t|t-1] C[t]'+R[t])
%       y[t|t-1] = C[t] x[t|t-1] + D[t] u[t]
%       x[t|t] = x[t|t-1] + L[t] (y[t] - C[t] x[t|t-1] - D[t] u[t])
%       P[t|t] = (I-L[t]C[t]) P[t|t-1]
%       y[t|t] = C[t] x[t|t] + D[t] u[t]
%
% Input: P should have the following properties at step t --- 
%   A, B, C, D: denote A[t], B[t], C[t], D[t]
%   Q, R: denote covariance matrices Q[t], R[t]
%   X, Y, U: denote x[t], y[t], u[t]
%   X_pre, Y_pre, P_pre: denote x[t|t-1], y[t|t-1], P[t|t-1]
%   X_est, L_est, P_est: denote x[t|t], L[t], P[t|t]
%
% Output: 
%   X_pre, Y_pre, P_pre: denote x[t|t-1], y[t|t-1], P[t|t-1]
%   X_est, L_est, P_est: denote x[t|t], L[t], P[t|t]
%   
% EXAMPLE:
%
%   P1=newplayer('pos',randn(1,2),'motion','random','speed',10,'legend','r')
%   P2=newplayer('pos',randn(1,2),'motion','random','speed',2,'legend','b')
%   G=newgame({P1,P2},'max_step',100,'pause','pause;')
%   G=simgame(G)
%
%  The above example simulates a game with two players with random motion.
%  In this game, players have random initial position, different speeds and colors. 
%  More interesting examples can be seen by running DEMO_MADS.M or EX_*.M.
%   
% See also NEWGAME, SIMGAME

%   Author: Hongbin Ma
%   Email:  mathmhb@163.com
%   Last updated: 04/02/09 with MATLAB 6.5
%
%   Problems or suggestions? Email me.


n=size(P.X,1);
m=size(P.Y,1);

%predict covariance matrix P at current step from last step estimation
P.P_pre=P.A*P.P_est*P.A'+P.Q;
%calculate Kalman filtering gain
tmp=P.P_pre*P.C';
P.L_est=tmp*inv(P.C*tmp+P.R);
%update estimation of covariance matrix P at current step
I=eye(n,n);
tmp2=[I-P.L_est*P.C];
%~ P.P_est=[I-P.L_est*P.C]*P.P_pre;
P.P_est=tmp2*P.P_pre*tmp2'+P.L_est*P.R*P.L_est';

%predict X at current step from last step estimation
P.X_pre=P.A*P.X_est+P.B*P.U;
%predict Y at current step from current prediction of X
P.Y_pre=P.C*P.X_pre+P.D*P.U;
%calculate innovation at current step
P.Y_err=P.Y-P.Y_pre;
%estimate X at current step using Kalman filtering gain
P.X_est=P.X_est+P.L_est*P.Y_err;
